{# ----------------------------------------------------------------------------
 # SymForce - Copyright 2022, Skydio, Inc.
 # This source code is under the Apache 2.0 license found in the LICENSE file.
 # ---------------------------------------------------------------------------- #}

{%- import "../util/util.jinja" as util with context -%}

#pragma once

#include <Eigen/Core>

namespace sym {

/**
{% for line in doc.cls.split('\n') %}
 *{{ ' {}'.format(line).rstrip() }}
{% endfor %}
 */
template <typename CameraCalType>
class Camera {
 public:
  using Scalar = typename CameraCalType::Scalar;

  Camera(const CameraCalType& calibration, const Eigen::Vector2i& image_size=Eigen::Vector2i(-1, -1))
    : calibration_(calibration),
      image_size_(image_size) {
  }

  Eigen::Matrix<Scalar, 2, 1> FocalLength() const {
    return calibration_.FocalLength();
  }

  Eigen::Matrix<Scalar, 2, 1> PrincipalPoint() const {
    return calibration_.PrincipalPoint();
  }

  {{ util.print_docstring(doc.pixel_from_camera_point) | indent(2) }}
  Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPoint(const Eigen::Matrix<Scalar, 3, 1>& point, const Scalar epsilon, Scalar* const is_valid) const {
    const Eigen::Matrix<Scalar, 2, 1> pixel = calibration_.PixelFromCameraPoint(point, epsilon, is_valid);
    if (is_valid != nullptr) {
      *is_valid *= MaybeCheckInView(pixel);
    }
    return pixel;
  }
  {{ util.print_docstring(doc.camera_ray_from_pixel) | indent(2) }}
  Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixel(const Eigen::Matrix<Scalar, 2, 1>& pixel, const Scalar epsilon, Scalar* const is_valid) const {
    const Eigen::Matrix<Scalar, 3, 1> camera_ray = calibration_.CameraRayFromPixel(pixel, epsilon, is_valid);
    if (is_valid != nullptr) {
      *is_valid *= MaybeCheckInView(pixel);
    }
    return camera_ray;
  }
  {{ util.print_docstring(doc.maybe_check_in_view) | indent(2) }}
  Scalar MaybeCheckInView(const Eigen::Matrix<Scalar, 2, 1>& pixel) const {
    if(image_size_[0] <= 0 || image_size_[1] <= 0) {
      // image size is not defined, don't check if the pixel is in view
      return 1;
    }
    return InView(pixel, image_size_);
  }
  {{ util.print_docstring(doc.in_view) | indent(2) }}
  static Scalar InView(const Eigen::Matrix<Scalar, 2, 1>& pixel, const Eigen::Matrix<int, 2, 1>& image_size) {
    const bool x_in_view = (pixel[0] >= 0) && (pixel[0] <= image_size[0] - 1);
    const bool y_in_view = (pixel[1] >= 0) && (pixel[1] <= image_size[1] - 1);
    return (x_in_view && y_in_view) ? 1 : 0;
  }

  CameraCalType Calibration() const {
    return calibration_;
  }

  Eigen::Matrix<int, 2, 1> ImageSize() const {
    return image_size_;
  }

  private:
    CameraCalType calibration_;
    Eigen::Matrix<int, 2, 1> image_size_;
};

}  // namespace sym
